package sinalgo.nodes;

import java.util.HashMap;

import projects.ufam.CustomGlobal;
import sinalgo.tools.statistics.GaussianDistribution;

public abstract class LocalizableNode extends Node{
	/**
	 * Distâncias para os vizinhos
	 */
	protected HashMap<Node, Double> neighbors = new HashMap<Node, Double>();
	public abstract Position getEstimatedPosition();
	public abstract boolean isReferenceNode();
	public Double estimateDistance(Node node)
	{
		Double result = getPosition().distanceTo( node.getPosition() );
		result += GaussianDistribution.nextGaussian( result*CustomGlobal.noiseMean(), result*CustomGlobal.noiseVariance());
		return result;
	}
	
	protected double residual( LocalizableNode[] references, Double[] distances ) {
		int n = references.length;
		double residual = 0;
		for( int j=0; j<n; j++) {
			double xr = references[j].getEstimatedPosition().xCoord;
			double yr = references[j].getEstimatedPosition().yCoord;
			double zr = references[j].getEstimatedPosition().zCoord;
			
			double x = this.getEstimatedPosition().xCoord;
			double y = this.getEstimatedPosition().yCoord;
			double z = this.getEstimatedPosition().zCoord;
			
			residual += Math.pow( Math.sqrt( Math.pow(xr-x, 2) + Math.pow(yr-y, 2) + Math.pow(zr-z, 2) ) - distances[j], 2 );
			residual /= n;
		}
		return residual;
	}
}
